/**************************************************************************
*
*	calibrate.c
*
*  Copyright 2007,2008,2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides the main functions for hardware calibration 
*
*
***************************************************************************
*
*  version 1.0, 2/22/09, SBS
*    - first version, derived from platform.c
*
*
***************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup platform
*  @brief
*  This module provides the main functions for hardware calibration 
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <termios.h>
#include <math.h>

#include <libgnome/gnome-config.h>

#include <time.h>
#include <sys/timeb.h>
#include "delay.h"

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"

#include "sd84.h"
#include "compass.h"


#define SLEEPDELAY (0.05)
#define COMPASSPORT	"/dev/compass0"

#define SERVO_PORT_THROTTLE 2
#define SERVO_PORT_RUDDER 4


static struct termios t_old;

/**
***************************************************************************
*
* mainloop()
*/
/** 
*
* @brief
*  Loops forever, updating telemetry and commands
*
*  Parameters:
*    - cf - configuration data struct
*
*  Returns:
*    - none
*
**************************************************************************/
static void mainloop(void)
{
	struct timeb starttime,endtime;
	TIMESPEC delay;
	double delta;
	struct termios t_new;	
	char inchar;
	double sleeptime;
	STATUS s;
	int polarity,centeroffset,positivescalefactor,negativescalefactor;
	
	s = status_InitStatus();
	
	delay = delay_setdelay(SLEEPDELAY);

	tcgetattr (1, &t_new);
	t_old = t_new;
	t_new.c_lflag &= ~(ICANON | ECHO);
	tcsetattr (1, 0, &t_new);

	delay = delay_setdelay(SLEEPDELAY);

	printf("Platform hardware calibration\n"
		"\n");
	
	printf("===============\n"
		"Next - calibrating rudder polarity\n"
		"\n");
	printf("Position the boat where it has plenty of room"
		" ahead and to either side.\n"
		" Press 'g' to continue"
		" or press 'x' to skip.\n");

	do{
		inchar = getchar ();
	}while(inchar != 'x' && inchar != 'g');
	
	if(inchar == 'g'){
	
		printf("Started!");
		
		double heading0;
		double heading1;
		double headingc;
		double diff1, diff2;
		sleeptime = 2.0;

		while(1){
		compass_GetStatus(&s);
		heading0 = s.dHdgDeg;
			
		sd84_set_servo(SERVO_PORT_THROTTLE,0);
		sd84_set_servo(SERVO_PORT_RUDDER,20);

		printf("here\n");
		
		ftime(&starttime);

		do{
			delay_sleep(&delay);
			ftime(&endtime);	
			delta = delay_deltatime(starttime,endtime);		
		}while(delta < sleeptime);
	
		printf("here2\n");
		
		compass_GetStatus(&s);
		heading1 = s.dHdgDeg;
		
		ftime(&starttime);

		do{
			delay_sleep(&delay);
			ftime(&endtime);	
			delta = delay_deltatime(starttime,endtime);		
		}while(delta < sleeptime);

		sd84_set_servo(SERVO_PORT_THROTTLE,0);
		sd84_set_servo(SERVO_PORT_RUDDER,0);
		
		compass_GetStatus(&s);
		headingc = s.dHdgDeg;

		printf("\t Initial heading = %lf\n",heading0);
		printf("\t Final heading = %lf\n",heading1);
		
		diff1 = heading1 - heading0;
		diff2 = headingc - heading1;

		heading0 += 720;
		heading1 += 720;
		
		if(heading1 > heading0){
			polarity = 1;
		}else{
			polarity = -1;
		}
		
		if (diff1/fabs(diff1) != diff2/fabs(diff2))
			continue;
		else
			break;
			
	}}

	printf("===============\n"
		"Next - calibrating rudder center position\n"
		"\n");
	printf("Position the boat where it has maximum forward space.\n"
		" Press 'g' to continue"
		" or press 'x' to skip.\n");

	do{
		inchar = getchar ();
	}while(inchar != 'x' && inchar != 'g');
	
	if(inchar == 'g'){
	
		printf("Started!");
		
		double heading0;
		double heading1;
		double headingc;
		double diff1, diff2;
		int rudder = 0;
		int rudderstep = 10;
		
		sleeptime = 2.5;

		while(1){
		
			compass_GetStatus(&s);
			headingc = s.dHdgDeg;
		
			sd84_set_servo(SERVO_PORT_THROTTLE,50);
			sd84_set_servo(SERVO_PORT_RUDDER,rudder);

			ftime(&starttime);
	
			do{
				delay_sleep(&delay);
				ftime(&endtime);	
				delta = delay_deltatime(starttime,endtime);		
			}while(delta < sleeptime);
	
			compass_GetStatus(&s);
			heading0 = s.dHdgDeg;
			
			ftime(&starttime);
	
			do{
				delay_sleep(&delay);
				ftime(&endtime);	
				delta = delay_deltatime(starttime,endtime);		
			}while(delta < sleeptime);
	
			compass_GetStatus(&s);
			heading1 = s.dHdgDeg;

			sd84_set_servo(SERVO_PORT_THROTTLE,0);
			sd84_set_servo(SERVO_PORT_RUDDER,rudder);
		
			printf("\t Initial heading = %lf\n",heading0);
			printf("\t Final heading = %lf\n",heading1);
			
			diff1 = headingc - heading0;
			diff2 = heading0 - heading1;

			if (diff1/fabs(diff1) != diff2/fabs(diff2))
				continue;

			heading0 += 720.;
			heading1 += 720.;
		
			if(fabs(heading1 - heading0) > 5.0){
				if(heading1 > heading0){
					rudder -= rudderstep;
				}else{
					rudder += rudderstep;
				}
				
				rudderstep /= 2;
				if(rudderstep < 2){
					rudderstep = 2;		
				}
			}else{
				break;
			}
		}		

		centeroffset = rudder;
		
	}
	
	positivescalefactor = 100;

	
	negativescalefactor = -100;
	
	tcsetattr (1, 0, &t_old);


	gnome_config_set_int("=calibration.cfg=/rudder/polarity"
			, polarity);
	gnome_config_set_int("=calibration.cfg=/rudder/centeroffset"
			, centeroffset);
	gnome_config_set_int("=calibration.cfg=/rudder/positivescalefactor"
			, positivescalefactor);
	gnome_config_set_int("=calibration.cfg=/rudder/negativescalefactor"
			, negativescalefactor);

	gnome_config_sync();


	return;
}


/**
***************************************************************************
*
* InterruptCallback()
*/
/** 
*
* @brief
*
* 
**************************************************************************/
void InterruptCallback(
			int signum)
{
	printf("\nExiting on signal %d!", signum);

	sd84_set_servo(SERVO_PORT_THROTTLE,0);
	sd84_set_servo(SERVO_PORT_RUDDER,0);

	tcsetattr (1, 0, &t_old);

	exit(signum);
}


/**
***************************************************************************
*
* SetIntCallback()
*/
/** 
*
* @brief
*
* 
*
**************************************************************************/
static void SetIntCallback()
{
	signal(SIGINT, InterruptCallback);
	signal(SIGTERM, InterruptCallback);
	signal(SIGQUIT, InterruptCallback);
	signal(SIGHUP, InterruptCallback);
}


/**
***************************************************************************
*
* main()
*/
/** 
*
* @brief
*  Entry point for program.  Reads configuration from
*    command line and config file, opens log file, 
*    then hands off to mainloop()
*
*  Parameters:
*    - argc
*    - argv
*
*  Returns:
*    - not used
*
**************************************************************************/
int main(
			int argc
			,char **argv)
{
	srand(time(NULL));

	SetIntCallback();

	sd84_Start("/dev/sd84_0");

	sd84_set_mode_servo(SERVO_PORT_RUDDER);
	sd84_set_mode_servo(SERVO_PORT_THROTTLE);

	compass_Start(COMPASSPORT);
	
	mainloop();
	
//	sd84_set_servo(SERVO_PORT_RUDDER,-43);
	
	return(0);
}

